#include <ros/ros.h>
#include "executor/chassis_executor.h"
#include "executor/gimbal_executor.h"
#include "behavior_tree/behavior_node.h"
#include "behavior_tree/behavior_tree.h"

#include "example_behavior/back_boot_area_behavior.h"//go 0 0 0
#include "example_behavior/escape_behavior.h"
#include "example_behavior/chase_behavior.h"
#include "example_behavior/search_behavior.h"
#include "example_behavior/patrol_behavior.h"
#include "example_behavior/goal_behavior.h"

//#include "example_behavior/aim_behavior.h"
#include "example_behavior/attack_behavior.h"
#include "example_behavior/bfs_search.h"
//#include "example_behavior/blood_behavior.h"
#include "example_behavior/buff_behavior.h"
#include "example_behavior/circle_behavior.h"
#include "example_behavior/dance_behavior.h"
//#include "example_behavior/gimbal_aim_behavior.h"
//#include "example_behavior/gimbalget0_behavior.h"
//#include "example_behavior/gogoal_behavior.h"
#include "example_behavior/recovery.h"
#include "example_behavior/shake_behavior.h"
#include "example_behavior/under_attack_behavior.h"
#include "example_behavior/wait_behavior.h"
//#include "example_behavior/cover_behavior.h"
#include "example_behavior/support_behavior.h"
#include <iostream>
#include "roborts_msgs/FricWhl.h"
#include "example_behavior/prepare_behavior.h"
#include "example_behavior/attack_enemy_buff_behavior.h"
#include "example_behavior/camera_behavior.h"
void param_init(ros::NodeHandle& p){}

int main(int argc, char **argv){
    ros::init(argc, argv, "tree_used_node");
	std::string full_path = ros::package::getPath("roborts_decision") + "/config/decision.prototxt";
	ros::NodeHandle nh_p("~") ;
	param_init(nh_p);
    auto blackboard_ptr_ = std::make_shared<roborts_decision::Blackboard>(full_path);
    roborts_decision::Blackboard* blackboard = &*blackboard_ptr_;
    //blackboard_ptr_->TurnMyId(blackboard_ptr_->GetRobotStatusinfo().id);//1 2 3 4
    auto bfs_search = new roborts_decision::BfsSearch(blackboard);
    auto chassis_executor_ptr_ = std::make_shared<roborts_decision::ChassisExecutor>();
    chassis_executor_ptr_->Set_Search(bfs_search);//
    auto gimbal_executor_ptr_ = std::make_shared<roborts_decision::GimbalExecutor>();

    //auto aim_action_=std::make_shared<roborts_decision::AimAction>(blackboard_ptr_, chassis_executor_ptr_);;
    auto attack_action_=std::make_shared<roborts_decision::AttackAction>(blackboard_ptr_);
    auto blood_buff_action_=std::make_shared<roborts_decision::BloodBuffAction>(blackboard_ptr_, chassis_executor_ptr_ );
    auto bullet_buff_action_=std::make_shared<roborts_decision::BulletBuffAction>(blackboard_ptr_, chassis_executor_ptr_ );
    auto chase_action_=std::make_shared<roborts_decision::ChaseAction>(blackboard_ptr_,chassis_executor_ptr_);
    auto support_action_=std::make_shared<roborts_decision::SupportAction>(blackboard_ptr_,chassis_executor_ptr_);
    auto circle_action_ = std::make_shared<roborts_decision::CircleAction>(blackboard_ptr_,chassis_executor_ptr_);
    auto dance_action_=std::make_shared<roborts_decision::DanceAction>(blackboard_ptr_, chassis_executor_ptr_, gimbal_executor_ptr_);
    auto patrol_action_ = std::make_shared<roborts_decision::PatrolAction>(blackboard_ptr_, chassis_executor_ptr_);
    auto recovery_action_=std::make_shared<roborts_decision::RecoveryAction>(chassis_executor_ptr_, blackboard_ptr_);
    auto shake_action_=std::make_shared<roborts_decision::ShakeAction>(blackboard_ptr_, gimbal_executor_ptr_);
    auto under_attack_action_ =std::make_shared<roborts_decision::UnderAttackAction>(blackboard_ptr_, chassis_executor_ptr_);
    auto wait_action_= std::make_shared<roborts_decision::WaitAction>(blackboard_ptr_,chassis_executor_ptr_, gimbal_executor_ptr_);
    auto prepare_action_=std::make_shared<roborts_decision::PrepareAction>(blackboard_ptr_, chassis_executor_ptr_,gimbal_executor_ptr_);
    auto attack_enemy_buff_action_=std::make_shared<roborts_decision::AttackEnemyBuffAction>(blackboard_ptr_, chassis_executor_ptr_);
    auto camera_action_=std::make_shared<roborts_decision::CameraAction>(blackboard_ptr_, chassis_executor_ptr_);
   /* auto aim_parallel_=std::make_shared<roborts_decision::ParallelNode>("aim parallel", blackboard_ptr_, 1);
    aim_parallel_->AddChildren(aim_action_);
    aim_parallel_->AddChildren(shake_action_);*/

    auto blood_buff_parallel_=std::make_shared<roborts_decision::ParallelNode>("blood buff parallel", blackboard_ptr_, 1);
    blood_buff_parallel_->AddChildren(blood_buff_action_);
    blood_buff_parallel_->AddChildren(shake_action_);

    auto bullet_buff_parallel_=std::make_shared<roborts_decision::ParallelNode>("bullet buff parallel", blackboard_ptr_, 1);
    bullet_buff_parallel_->AddChildren(bullet_buff_action_);
    bullet_buff_parallel_->AddChildren(shake_action_);

    auto patrol_parallel_=std::make_shared<roborts_decision::ParallelNode>("patrol parallel", blackboard_ptr_, 1);
    patrol_parallel_->AddChildren(patrol_action_);
    patrol_parallel_->AddChildren(shake_action_);

    auto underattack_parallel_=std::make_shared<roborts_decision::ParallelNode>("under attack parallel", blackboard_ptr_, 1);
    patrol_parallel_->AddChildren(under_attack_action_);
    patrol_parallel_->AddChildren(shake_action_);
 /*   auto aim_condition_=std::make_shared<roborts_decision::PreconditionNode>("aim condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->IsCameraFindEnemy())return true;//find >0.5
        else return false;
    }, roborts_decision::AbortType::BOTH);
    aim_condition_->SetChild(aim_parallel_);*/

    auto attack_prepare_parallel_=std::make_shared<roborts_decision::ParallelNode>("attack prepare parallel", blackboard_ptr_, 1);
    attack_prepare_parallel_->AddChildren(prepare_action_);
    attack_prepare_parallel_->AddChildren(attack_action_);

    auto attack_prepare_condition_=std::make_shared<roborts_decision::PreconditionNode>("attack prepare condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->attack_condition())return true;//by if_shoot_ from roborts_all
        else return false;
    }, roborts_decision::AbortType::BOTH);
    attack_prepare_condition_->SetChild(attack_prepare_parallel_);

    auto attack_dance_parallel_=std::make_shared<roborts_decision::ParallelNode>("attack dance parallel", blackboard_ptr_, 1);
    attack_dance_parallel_->AddChildren(dance_action_);
    attack_dance_parallel_->AddChildren(attack_action_);

    auto attack_dance_condition_=std::make_shared<roborts_decision::PreconditionNode>("attack dance condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->attack_condition())return true;//by if_shoot_ from roborts_all
        else return false;
    }, roborts_decision::AbortType::BOTH);
    attack_dance_condition_->SetChild(attack_dance_parallel_);
    
    auto attack_enemy_buff_condition_=std::make_shared<roborts_decision::PreconditionNode>("attack enemy buff condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->attack_enemy_buff_condition())return true;
        else return false;
}, roborts_decision::AbortType::BOTH);
        attack_enemy_buff_condition_->SetChild(attack_enemy_buff_action_);

    auto camera_condition_=std::make_shared<roborts_decision::PreconditionNode>("camera condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->IsCameraFindEnemy())return true;
        else return false;
    }, roborts_decision::AbortType::BOTH);
      camera_condition_->SetChild(camera_action_);

  /*  auto underattack_fivesec_condition_=std::make_shared<roborts_decision::PreconditionNode>("attack condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->attack_condition())return true;//by if_shoot_ from roborts_all
        else return false;
    }, roborts_decision::AbortType::BOTH);
    attack_condition_->SetChild(attack_parallel_);*/

  /*  auto blood_danger_condition_=std::make_shared<roborts_decision::PreconditionNode>("blood danger condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->GetMyHpRecoveryBuffInfo()&&blackboard_ptr_->GetGameRobotHPinfo()<200)return true;//from system
        else return false;
    }, roborts_decision::AbortType::BOTH);
    blood_danger_condition_->SetChild(blood_buff_parallel_);*/

    auto circle_condition_=std::make_shared<roborts_decision::PreconditionNode>("circle condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->circle_condition())return true;//from system
        else return false;
    }, roborts_decision::AbortType::BOTH);
    circle_condition_->SetChild(circle_action_);

    auto blood_add_condition_=std::make_shared<roborts_decision::PreconditionNode>("blood add condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->blood_add_condition())return true;
        else return false;
    }, roborts_decision::AbortType::BOTH);
    blood_add_condition_->SetChild(blood_buff_parallel_);

    auto bullet_buff_condition_=std::make_shared<roborts_decision::PreconditionNode>("bullet buff condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->GetMyBulletSupplyBuffInfo())return true;
        else return false;
    }, roborts_decision::AbortType::BOTH);
    bullet_buff_condition_->SetChild(bullet_buff_parallel_);

    auto chase_condition_=std::make_shared<roborts_decision::PreconditionNode>("chase condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->chase_condition())return true;
        else return false;//from roborts_all
    }, roborts_decision::AbortType::BOTH);
    chase_condition_->SetChild(chase_action_);

    auto support_condition_=std::make_shared<roborts_decision::PreconditionNode>("support condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->cover_condition())return true;
        else return false;//from roborts_all
    }, roborts_decision::AbortType::BOTH);
    support_condition_->SetChild(support_action_);

    /*auto chase_parallel_=std::make_shared<roborts_decision::ParallelNode>("chase parallel", blackboard_ptr_, 1);
    chase_parallel_->AddChildren(chase_action_);
    chase_parallel_->AddChildren(attack_action_);*/

    /*auto chase_attack_condition_=std::make_shared<roborts_decision::PreconditionNode>("chase attack condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->GetIfEnemy().data&&
        blackboard_ptr_->GetDistance(blackboard_ptr_->GetRobotMapPose(), blackboard_ptr_->GetAimPosition())>2)return true;
        else return false;
    }, roborts_decision::AbortType::BOTH);
    chase_condition_->SetChild(chase_parallel_);*/

  auto prepare_selector_=std::make_shared<roborts_decision::SelectorNode>("prepare selector",blackboard_ptr_);
   // dance_selector_->AddChildren(underattack_fivesec_condition_);
    prepare_selector_->AddChildren(attack_prepare_condition_);
    prepare_selector_->AddChildren(prepare_action_);

  auto prepare_condition_=std::make_shared<roborts_decision::PreconditionNode>("prepare condition" , blackboard_ptr_, [&](){
        if(blackboard_ptr_->PrepareCondition())return true;
        else return false;
    }, roborts_decision::AbortType::BOTH);
    prepare_condition_->SetChild(prepare_selector_);

    auto dance_selector_=std::make_shared<roborts_decision::SelectorNode>("dance selector",blackboard_ptr_);
   // dance_selector_->AddChildren(underattack_fivesec_condition_);
    dance_selector_->AddChildren(attack_dance_condition_);
    dance_selector_->AddChildren(dance_action_);

    auto dance_condition_=std::make_shared<roborts_decision::PreconditionNode>("dance condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->GetIfFindEnemy())return true;//if enemy +1s
        else return false;
    }, roborts_decision::AbortType::BOTH);
    dance_condition_->SetChild(dance_selector_);

    auto recovery_condition_=std::make_shared<roborts_decision::PreconditionNode>("recovery condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->RecoveryCondition())return true;// ! 1->free 0->jam
        else return false;
    }, roborts_decision::AbortType::BOTH);
    recovery_condition_->SetChild(recovery_action_);

    auto under_attack_condition_=std::make_shared<roborts_decision::PreconditionNode>("under attack condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->IsUnderAttack())return true;//dose well
        else return false;
    }, roborts_decision::AbortType::BOTH);
    under_attack_condition_->SetChild(underattack_parallel_);

    auto no_fire_selector_=std::make_shared<roborts_decision::SelectorNode>("no fire selector",blackboard_ptr_);
    no_fire_selector_->AddChildren(bullet_buff_condition_);
    no_fire_selector_->AddChildren(blood_add_condition_);
    no_fire_selector_->AddChildren(circle_action_);//circle和回家哪个合适?

    auto no_fire_condition_=std::make_shared<roborts_decision::PreconditionNode>("no fire condition", blackboard_ptr_, [&](){
        if(!blackboard_ptr_->IsBulletEnough()) return true;// ! false->no fire, true->have 2+
        else return false;
    }, roborts_decision::AbortType::BOTH);
    no_fire_condition_->SetChild(no_fire_selector_);

    auto main_selector_=std::make_shared<roborts_decision::SelectorNode>("main selector",blackboard_ptr_);
    main_selector_->AddChildren(recovery_condition_);
    main_selector_->AddChildren(no_fire_condition_);
    //main_selector_->AddChildren(blood_danger_condition_);
    //main_selector_->AddChildren(circle_condition_);
    main_selector_->AddChildren(chase_condition_);
    //main_selector_->AddChildren(chase_attack_condition_);
    main_selector_->AddChildren(prepare_condition_);//not sound, need to check
    main_selector_->AddChildren(dance_condition_); 
    main_selector_->AddChildren(under_attack_condition_);
    main_selector_->AddChildren(support_condition_);
    if(blackboard_ptr_->GetMyId()==1||blackboard_ptr_->GetMyId()==3) {
        main_selector_->AddChildren(attack_enemy_buff_condition_);
    }
        if(blackboard_ptr_->GetMyId()==2||blackboard_ptr_->GetMyId()==4) {
        main_selector_->AddChildren(bullet_buff_condition_);
      main_selector_->AddChildren(blood_add_condition_);
    }
   //main_selector_->AddChildren(blood_add_condition_);
    //main_selector_->AddChildren(aim_condition_);
    main_selector_->AddChildren(camera_condition_);
    main_selector_->AddChildren(patrol_parallel_);

    auto game_start_condition_=std::make_shared<roborts_decision::PreconditionNode>("game start condition", blackboard_ptr_, [&](){
        if(blackboard_ptr_->start_condition()) return true;//dose well
        else return false;
    }, roborts_decision::AbortType::BOTH);
    game_start_condition_->SetChild(main_selector_);

    auto root_selector_ = std::make_shared<roborts_decision::SelectorNode>("root",blackboard_ptr_);
    root_selector_->AddChildren(game_start_condition_);
    root_selector_->AddChildren(wait_action_);
    roborts_decision::BehaviorTree root(root_selector_,10);
    
    ros::service::waitForService("cmd_fric_wheel");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<roborts_msgs::FricWhl>("cmd_fric_wheel");
    roborts_msgs::FricWhl Fric;
    Fric.request.open = true;
    if(!client.call(Fric))
        ROS_ERROR("FRICWHEEL NOT OPEN");
    sleep(3);
    
    root.Run();
    return 0;
}
